/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */
#pragma once

#include <mrpt/core/bits_math.h>
#include <mrpt/core/format.h>
#include <mrpt/math/TPoseOrPoint.h>
#include <cmath>
#include <vector>

namespace mrpt::math
{
/** \ingroup geometry_grp */
struct TPoint2D_data
{
	/** X,Y coordinates */
	double x, y;
};

/**
 * Lightweight 2D point. Allows coordinate access using [] operator.
 * \sa mrpt::poses::CPoint2D
 * \ingroup geometry_grp
 */
struct TPoint2D : public TPoseOrPoint,
				  public TPoint2D_data,
				  public internal::ProvideStaticResize<TPoint2D>
{
	enum
	{
		static_size = 2
	};
	/** Default constructor. Initializes to zeros  */
	constexpr TPoint2D() : TPoint2D_data{0, 0} {}
	/** Constructor from coordinates  */
	constexpr TPoint2D(double xx, double yy) : TPoint2D_data{xx, yy} {}
	constexpr TPoint2D(const TPoint2D_data& d) : TPoint2D_data{d.x, d.y} {}

	/** Constructor from TPose2D, discarding phi.
	 * \sa TPose2D
	 */
	explicit TPoint2D(const TPose2D& p);
	/**
	 * Constructor from TPoint3D, discarding z.
	 * \sa TPoint3D
	 */
	explicit TPoint2D(const TPoint3D& p);
	/**
	 * Constructor from TPose3D, discarding z and the angular coordinates.
	 * \sa TPose3D
	 */
	explicit TPoint2D(const TPose3D& p);

	/** Coordinate access using operator[]. Order: x,y */
	double& operator[](size_t i)
	{
		switch (i)
		{
			case 0:
				return x;
			case 1:
				return y;
			default:
				throw std::out_of_range("index out of range");
		}
	}
	/** Coordinate access using operator[]. Order: x,y */
	constexpr const double& operator[](size_t i) const
	{
		switch (i)
		{
			case 0:
				return x;
			case 1:
				return y;
			default:
				throw std::out_of_range("index out of range");
		}
	}

	/**
	 * Transformation into vector.
	 */
	void asVector(std::vector<double>& v) const
	{
		v.resize(2);
		v[0] = x;
		v[1] = y;
	}

	bool operator<(const TPoint2D& p) const;

	TPoint2D& operator+=(const TPoint2D& p)
	{
		x += p.x;
		y += p.y;
		return *this;
	}

	TPoint2D& operator-=(const TPoint2D& p)
	{
		x -= p.x;
		y -= p.y;
		return *this;
	}

	TPoint2D& operator*=(double d)
	{
		x *= d;
		y *= d;
		return *this;
	}

	TPoint2D& operator/=(double d)
	{
		x /= d;
		y /= d;
		return *this;
	}

	constexpr TPoint2D operator+(const TPoint2D& p) const
	{
		return {x + p.x, y + p.y};
	}

	constexpr TPoint2D operator-(const TPoint2D& p) const
	{
		return {x - p.x, y - p.y};
	}

	constexpr TPoint2D operator*(double d) const { return {d * x, d * y}; }
	constexpr TPoint2D operator/(double d) const { return {x / d, y / d}; }
	/** Returns a human-readable textual representation of the object (eg:
	 * "[0.02 1.04]" )
	 * \sa fromString
	 */
	void asString(std::string& s) const { s = mrpt::format("[%f %f]", x, y); }
	std::string asString() const
	{
		std::string s;
		asString(s);
		return s;
	}

	/** Set the current object value from a string generated by 'asString' (eg:
	 * "[0.02 1.04]" )
	 * \sa asString
	 * \exception std::exception On invalid format
	 */
	void fromString(const std::string& s);

	/** Squared norm: |v|^2 = x^2+y^2 */
	double sqrNorm() const { return x * x + y * y; }

	/** Point norm: |v| = sqrt(x^2+y^2) */
	double norm() const { return std::sqrt(sqrNorm()); }
};

/** Exact comparison between 2D points */
constexpr bool operator==(const TPoint2D& p1, const TPoint2D& p2)
{
	return (p1.x == p2.x) && (p1.y == p2.y);  //-V550
}
/**  Exact comparison between 2D points */
constexpr bool operator!=(const TPoint2D& p1, const TPoint2D& p2)
{
	return (p1.x != p2.x) || (p1.y != p2.y);  //-V550
}

}  // namespace mrpt::math

namespace mrpt::typemeta
{
// Specialization must occur in the same namespace
MRPT_DECLARE_TTYPENAME_NO_NAMESPACE(TPoint2D, mrpt::math)
}  // namespace mrpt::typemeta
